#include "pid.h"

#define SIGMA 0.000001f

/**
 * @brief  PID内部初始化
 * @param  无
 * @retval 无
 */
void pid_init(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
	if (pid_set_parameters(pid, kp, ki, kd, integral_limit, output_limit)) {
		
	}
	
	pid->Derror      	= 0;
	pid->DDerror     	= 0;
	pid->last_Derror  = 0;
  pid->integral    = 0;
	pid->last_error  	= 0;
}

/**
 * @brief  pid设置参数
 * @param  void
 * @retval void
 */
int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
	int ret = 0;

	if (isfinite(kp)) {
		pid->kp = kp;

	} else {
		ret = 1;
	}

	if (isfinite(ki)) {
		pid->ki = ki;

	} else {
		ret = 1;
	}

	if (isfinite(kd)) {
		pid->kd = kd;

	} else {
		ret = 1;
	}

	if (isfinite(integral_limit)) {
		pid->integral_limit = integral_limit;

	}  else {
		ret = 1;
	}

	if (isfinite(output_limit)) {
		pid->output_limit = output_limit;

	}  else {
		ret = 1;
	}

	return ret;
}

/**
 * @brief  复位积分
 * @param  void
 * @retval void
 */
void pid_reset_integral(PID_t *pid)
{
	pid->integral = 0.0f;
}

/**
 * @brief  数据取反
 * @param  无
 * @retval 无
 */
static float abs_limit(float * a, float * ABS_MAX)
{
	if(*a > *ABS_MAX)
		*a = *ABS_MAX;

	if(*a < -*ABS_MAX)
		*a = -*ABS_MAX;
	return *a;
}

/**
  * @pid            PID结构体
  * @ideal_value    期望值
  * @reality_value  实际值
  *返回值: 无
  */
float pid_calculate_position(PID_t *pid, float sp, float val, float dt)
{
	if ((!isfinite(sp) || !isfinite(val) || !isfinite(dt)) \
			|| (((sp - val) == pid->last_error))){
		return pid->last_output;
	}

	float i, d;

	/* current error value */
	float error = sp - val;
	
	/* current error derivative */
//	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
		d = (error - pid->last_error) / dt;
		pid->last_error = error;

//	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
//		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
//		pid->error_previous = -val;

//	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
//		d = -val_dot;

//	} else {
//		d = 0.0f;
//	}
	
	
	if (!isfinite(d)) {
		d = 0.0f;
	}
	
	/* calculate PD output */
	float output = (error * pid->kp) + (d * pid->kd);
	
	if (pid->ki > SIGMA) {
		// Calculate the error integral and check for saturation
		i = pid->integral + (error * dt);

		/* check for saturation */
		if (isfinite(i)) {
			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
			    fabsf(i) <= pid->integral_limit) {
				/* not saturated, use new integral value */
				pid->integral = i;
			}
		}

		/* add I component to output */
		output += pid->integral * pid->ki;
	}
	
	/* limit output */
	if (isfinite(output)) {
		if (pid->output_limit > SIGMA) {
			if (output > pid->output_limit) {
				output = pid->output_limit;

			} else if (output < -pid->output_limit) {
				output = -pid->output_limit;
			}
		}

		pid->last_output = output;
	}

	return pid->last_output;
}


/**
  * @pid            PID结构体
  * @ideal_value    期望值
  * @reality_value  实际值
  *返回值: 无
  */
float PID_Compute_Increm(PID_t *pid, float sp, float val)
{
	float output;
	float Error_I;
	
	/* current error value */
	float error = sp - val;
	
	pid->Derror = error - pid->last_error;						//	误差变化量 = 这一次误差 - 上一次误差
	pid->DDerror=pid->Derror -pid->last_Derror;				//  误差变化量的变化量 = 上一次误差变化量 - 这一次误差变化量
	
	Error_I = pid->ki * error;
	
	output  = 	(pid->kp * pid->Derror)+	\
							abs_limit(&Error_I, &pid->integral_limit) +  \
							(pid->kd * pid->DDerror)+  \
							pid->last_output;
	
	if(output > pid->output_limit)	output = pid->output_limit;
	if(output < -pid->output_limit)	output = -pid->output_limit;
	
	pid->last_output = output;
	pid->last_error  = error;
	pid->last_Derror = pid->Derror;
	
	return output;
}

/**
 * @brief  YAW限制
 * @param  void
 * @retval void
 */
float yaw_clamp( double control_yaw_error )
{
	for (int i = 0; i < 20; i++){
		if(control_yaw_error > 180.0f) {
			control_yaw_error -= 360.0f;
		} else if(control_yaw_error < -180.0f) {
			control_yaw_error += 360.0f;
		} else {
			break;
		}
	}

	return control_yaw_error;
}

/**
 * @brief  YAW轴转角计算
 * @param  void
 * @retval void
 */
float yaw_pid_updata(PID_t *pid, float control_yaw, float now_yaw, float Dtime)
{
	float control_yaw_error = control_yaw - now_yaw;
	
	if(control_yaw_error > 180.0f) {
		control_yaw_error -= 360.0f;
	} 
	if(control_yaw_error < -180.0f) {
		control_yaw_error += 360.0f;
	}
		
	return pid_calculate_position(pid,   control_yaw_error,  0, 	Dtime);
}

/**
 * @brief  PID积分清零
 * @param  无
 * @retval 无
 */
void PID_Reset(PID_t * PID)
{	
	//复位上一次误差
	PID->last_error = 0;
	
	//复位积分
	PID->integral = 0;
	
	//复位微分
	PID->Derror = 0;
	
	PID->DDerror = 0;
	PID->last_Derror = 0;
	PID->last_output = 0;
}




